#include "graph_optimizer/vertex/vertex_q.hpp"

namespace g2o
{
    VertexQ::VertexQ() : BaseVertex<3, Eigen::Quaterniond>() { setToOriginImpl(); }
    bool VertexQ::read(std::istream &is)
    {
        Eigen::Quaterniond lv;
        for (int i = 0; i < 4; i++)
            is >> lv.coeffs()[i];
        setEstimate(lv);
        return true;
    }

    bool VertexQ::write(std::ostream &os) const
    {
        Eigen::Quaterniond lv = estimate();
        for (int i = 0; i < 4; i++)
        {
            os << lv.coeffs()[i] << " ";
        }
        return os.good();
    }
} // namespace g2o